#include "Game Engine\Header\Physics.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"

Physics::Physics() :
mDefaultContactProcessingThreshold( BT_LARGE_FLOAT )
{
}

Physics::~Physics()
{
}

void Physics::Init()
{
}

// Creates a rigid body based on the mass, transform and shape
// and adds it to the dynamics world.
btRigidBody* Physics::CreateRigidBodyGeneric( btDynamicsWorld* world, Mesh* mesh,
	float mass, Shape shape )
{
	btCollisionShape* cs;

	//Create Motion State
	btQuaternion q( 0.0f, 0.0f, 0.0f );
	btVector3 p( 0.0f, 0.0f, 0.0f );
	btTransform startTrans( q, p );

	D3DXVECTOR3 min;
	D3DXVECTOR3 max;

	mesh->GetAABB( min, max );

	float sizeX = abs( max.x - min.x );
	float sizeY = abs( max.y - min.y );
	float sizeZ = abs( max.z - min.z );

	switch( shape )
	{
	case Capsule:
		{
			cs = new btCapsuleShape( sizeX, sizeY );
			break;
		}
	case Cube:
		{
			cs = new btBoxShape( btVector3( sizeX * 0.5f, sizeY * 0.5f, sizeZ * 0.5f ) );
			break;
		}
	case Cyl:
		{
			cs = new btCylinderShape( btVector3( CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS ) );
			break;
		}
	case Plane:
		{
			cs = new btStaticPlaneShape(btVector3(0,1,0),-0.5);
			break;
		}
	case Sph:
		{
			cs = new btSphereShape( sizeX * 0.5f );
			break;
		}
	case TriMesh:
		{
			btTriangleMesh* trimesh = new btTriangleMesh();

			//btVector3 localScaling(6.f,6.f,6.f);

			int i;
			for( i = 0; i < mesh->GetNumTriangles(); i++ )
			{
				int index0 = mesh->GetIndices()[i * 3];
				int index1 = mesh->GetIndices()[i * 3 + 1];
				int index2 = mesh->GetIndices()[i * 3 + 2];

				btVector3 vertex0( mesh->GetVertices()[index0].pos.x, mesh->GetVertices()[index0].pos.y, mesh->GetVertices()[index0].pos.z );
				btVector3 vertex1( mesh->GetVertices()[index1].pos.x, mesh->GetVertices()[index1].pos.y, mesh->GetVertices()[index1].pos.z );
				btVector3 vertex2( mesh->GetVertices()[index2].pos.x, mesh->GetVertices()[index2].pos.y, mesh->GetVertices()[index2].pos.z );

				//vertex0 *= localScaling;
				//vertex1 *= localScaling;
				//vertex2 *= localScaling;

				trimesh->addTriangle( vertex0, vertex1, vertex2 );
			}

			btConvexShape* tmpConvexShape = new btConvexTriangleMeshShape(trimesh);

			//create a hull approximation
			btShapeHull* hull = new btShapeHull(tmpConvexShape);
			btScalar margin = tmpConvexShape->getMargin();
			hull->buildHull(10.0f);
			tmpConvexShape->setUserPointer(hull);

			btConvexHullShape* convexShape = new btConvexHullShape();
			for (i=0;i<hull->numVertices();i++)
			{
				convexShape->addPoint(hull->getVertexPointer()[i]);	
			}

			//bool useQuantization = true;
			//btCollisionShape* concaveShape = new btBvhTriangleMeshShape(trimesh,useQuantization);

			cs = convexShape;

			delete tmpConvexShape;
			delete hull;

			break;
		}
	};

	btAssert( ( !cs || cs->getShapeType() != INVALID_SHAPE_PROXYTYPE ) );

	// Rigidbody is dynamic if and only if mass is non zero, otherwise static.
	//bool isDynamic = ( mass != 0.f );

	bool isDynamic = ( mass != 0.f );

	btVector3 localInertia( 0, 0, 0 );
	if( isDynamic )
		cs->calculateLocalInertia( mass, localInertia );

	// Using motionstate is recommended, it provides interpolation capabilities,
	// and only synchronizes 'active' objects.

#define USE_MOTIONSTATE 1
#ifdef USE_MOTIONSTATE
	btDefaultMotionState* myMotionState = new btDefaultMotionState( startTrans );

	btRigidBody::btRigidBodyConstructionInfo cInfo( mass, myMotionState, cs, localInertia );

	btRigidBody* body = new btRigidBody( cInfo );
	body->setContactProcessingThreshold( mDefaultContactProcessingThreshold );

	//mBody = new btRigidBody( mass, ms, cs, localInertia );

	body->setCcdMotionThreshold(CUBE_HALF_EXTENTS);
	body->setCcdSweptSphereRadius(0.9*CUBE_HALF_EXTENTS);
#else
	btRigidBody* body = new btRigidBody(mass, 0, shape, localInertia );	
	body->setWorldTransform( startTransform );
#endif//

	world->addRigidBody( body );

	return body;
}

btRigidBody* Physics::CreateRigidBody( btDynamicsWorld* world, float mass, const btTransform& startTransform, btCollisionShape* shape )
{
	btAssert( ( !shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE ) );

	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	bool isDynamic = ( mass != 0.f );

	btVector3 localInertia( 0, 0, 0 );
	if( isDynamic )
		shape->calculateLocalInertia( mass, localInertia );

	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects

#define USE_MOTIONSTATE 1
#ifdef USE_MOTIONSTATE
	btDefaultMotionState* myMotionState = new btDefaultMotionState( startTransform );

	btRigidBody::btRigidBodyConstructionInfo cInfo( mass, myMotionState, shape, localInertia );

	btRigidBody* body = new btRigidBody( cInfo );
	body->setContactProcessingThreshold( mDefaultContactProcessingThreshold );

#else
	btRigidBody* body = new btRigidBody( mass, 0, shape, localInertia );	
	body->setWorldTransform( startTransform );
#endif//

	world->addRigidBody( body );

	return body;
}

btPairCachingGhostObject* Physics::CreateGhostObject( btDynamicsWorld* world, Mesh* mesh,
	float mass,	Shape shape )
{
	D3DXVECTOR3 min;
	D3DXVECTOR3 max;

	mesh->GetAABB( min, max );

	float height = abs( max.y - min.y );
	float width = abs( max.x - min.x );
	float depth = abs( max.z - min.z );

	btTransform startTransform;
	startTransform.setIdentity();
	//startTransform.setOrigin ( btVector3( 0.0, 4.0, 0.0 ) );
	startTransform.setOrigin( btVector3( 0.0f, 0.0f, 0.0f ) );


	btPairCachingGhostObject* ghostObject = new btPairCachingGhostObject();
	ghostObject->setWorldTransform( startTransform );
	btScalar characterHeight = height/2 + 0.04f;
	btScalar characterWidth = width/2;
	btScalar characterDepth = depth/2;
	btConvexShape* capsule;
	//Create Collision Shape
	switch( shape )
	{
	case Capsule:
		{
			capsule = new btCapsuleShape( characterWidth, characterHeight );
			break;
		}
	case Cube:
		{
			capsule = new btBoxShape( btVector3( characterWidth, characterHeight, characterDepth ) ); //btCapsuleShape(characterWidth,characterHeight);
			break;
		}
	case Cyl:
		{
			capsule = new btCylinderShape( btVector3( CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS ) );
			break;
		}
	case Sph:
		{
			capsule = new btSphereShape( characterWidth );
			break;
		}
	default:
		{
			capsule = new btBoxShape( btVector3( characterWidth, characterHeight, characterDepth ) ); //btCapsuleShape(characterWidth,characterHeight);
			break;
		}
	};
	ghostObject->setCollisionShape ( capsule );
	ghostObject->setCollisionFlags ( btCollisionObject::CF_CHARACTER_OBJECT );

	return ghostObject;
}

btPairCachingGhostObject* Physics::CreateGhostObject( btDynamicsWorld* world, float mass,
	const btTransform& startTransform, btCollisionShape* shape )
{
	btPairCachingGhostObject* ghostObject = new btPairCachingGhostObject();
	ghostObject->setWorldTransform( startTransform );

	ghostObject->setCollisionShape ( shape );
	ghostObject->setCollisionFlags ( btCollisionObject::CF_CHARACTER_OBJECT );

	return ghostObject;
}